package controller;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.Sound;

public class NxtSafeState {

	// vars
	public static final NxtSafeState nxtSafeState = new NxtSafeState();
	public static boolean init = true;

	// constructors
	private NxtSafeState() {
		super();
	}

	// methods
	public static NxtSafeState getInstance() {
		return nxtSafeState;
	}

	public void putInSafeState() {
		if (init) {
			// old code (crappy crappy solution but time is running out...
			System.out.println("entering safe state");

			Motor.B.rotateTo(0, true);

			while (Motor.B.isMoving())
				Thread.yield();

			Motor.B.stop();

			System.out.println("Tacho: " + Motor.B.getTachoCount());
			if (Motor.B.getTachoCount() != 0) {
				System.out.println("Fixing rotation");

				while (Motor.B.getTachoCount() != 0) {
					System.out.println("TRYING 2");
					Motor.B.rotateTo(0, true);
					while (Motor.B.isMoving())
						Thread.yield();
				}
				Motor.B.stop();
			}

			System.out.println("press button to exit");
			Button.waitForAnyPress();
			System.exit(-1);
		} else {

			boolean ready = false;
			int sel = 1;
			LCD.clear();
			LCD.drawString("entering safe state", 0, 0);

			Sound.twoBeeps();

			Motor.B.rotateTo(0, true);

			while (Motor.B.isMoving())
				Thread.yield();

			Motor.B.stop();

			if (Motor.B.getTachoCount() != 0) {

				while (Motor.B.getTachoCount() != 0) {
					Motor.B.rotateTo(0, true);
					while (Motor.B.isMoving())
						Thread.yield();
				}
				Motor.B.stop();
			}

			LCD.drawString("Try to fix it and", 0, 1);
			LCD.drawString("choose an option:", 0, 2);
			LCD.drawString("Restart >", 0, 4);

			while (!ready) {
				int pressedButton = Button.waitForAnyPress();

				switch (pressedButton) {
				case Button.ID_LEFT:
					if (sel == 2) {
						sel = 1;
						LCD.clear(4);
						LCD.drawString("Restart >", 0, 4);
					}
					break;
				case Button.ID_RIGHT:
					if (sel == 1) {
						sel = 2;
						LCD.clear(4);
						LCD.drawString("< Exit", 0, 4);
					}
					break;
				case Button.ID_ENTER:
					ready = true;
					break;
				}

			}
			if (sel == 2)
				System.exit(-1);
		}
	}
}
